#include <fstream>
#include <iterator>
#include <numeric> //PM

#ifdef _OPENMP
#include <omp.h>
#endif

#include "graclus.h"
#include "bundle.h"
#define _USE_MATH_DEFINES
#include <math.h>

extern "C"
{
int boundary_points;
int spectral_initialization = 0;
int cutType;
int memory_saving;
};

using namespace std;
using namespace boost;
using namespace CMVS;

Cbundle::Cbundle(void) {
    m_CPU = 8;
    m_junit = 100;
    mtx_init(&m_lock, mtx_plain | mtx_recursive);
    m_debug = 0;
    m_puf = NULL;
    m_ptree = NULL;
}

Cbundle::~Cbundle() {
}

void Cbundle::prep(const std::string prefix, const int imageThreshold,
                   const int tau, const float scoreRatioThreshold,
                   const float coverageThreshold,
                   const int pnumThreshold, const int CPU) {
    if (pnumThreshold != 0) {
        cerr << "Should use pnumThreshold = 0" << endl;
        exit (1);
    }

    //path-prefix
    m_prefix = prefix;
    m_imageThreshold = imageThreshold;
    m_tau = tau;
    m_scoreRatioThreshold = scoreRatioThreshold;
    m_coverageThreshold = coverageThreshold;
    m_pnumThreshold = pnumThreshold;
    m_CPU = CPU;

    m_linkThreshold = 2.0f;

    m_dscale = 1 / 100.0f;
    m_dscale2 = 1.0f;

//    char buffer[1024];
//    sprintf(buffer, "%sbundle.rd.out", prefix.c_str());
//    cerr << "Reading bundle..." << flush;
//    readBundle(buffer);
//    cerr << endl;
    setNeighbors();
    cerr << endl;

    vector<int> images;
    for (int c = 0; c < m_cnum; ++c)
        images.push_back(c);

    m_dlevel = 7;
    m_maxLevel = 12;
    m_pss.init(images, prefix, m_maxLevel + 1, 5, 0,m_intri,m_extri);

    cerr << "Set widths/heights..." << flush;
    setWidthsHeightsLevels();
    cerr << "done" << flush;
}

void Cbundle::prep2(void) {
    // Used in mergeSfMP now.
    {
        m_pweights.resize((int)m_coords.size(), 1);
        m_sfms2.resize((int)m_coords.size());
        startTimer();
        setScoreThresholds();
        cerr << "done\t" << curTimer()/CLOCKS_PER_SEC << " secs" << endl;
        startTimer();
        cerr << "slimNeighborsSetLinks..." << flush;
        slimNeighborsSetLinks();
        cerr << "done\t" << curTimer()/CLOCKS_PER_SEC << " secs" << endl;
    }

    // Improve visibility by using texture analysis
    startTimer();
    cerr << "mergeSFM..." << flush;
    mergeSfMP();
    cerr << '\t' << curTimer()/CLOCKS_PER_SEC << " secs" << endl;

    m_sfms2.clear();
    m_sfms2.resize((int)m_coords.size());
    cerr << "setScoreThresholds..." << flush;
    startTimer();
    setScoreThresholds();
    cerr << "done\t" << curTimer()/CLOCKS_PER_SEC << " secs" << endl;

    // Remove redundant images first
    cerr << "sRemoveImages... " << flush;
    startTimer();

    sRemoveImages();
    cerr << '\t' << curTimer()/CLOCKS_PER_SEC << " secs" << endl;

    // use m_removed to change m_visibles and update m_neighbors
    startTimer();
    resetVisibles();
    setNeighbors();
    cerr << "slimNeighborsSetLinks..." << flush;
    slimNeighborsSetLinks();
    cerr << "done\t" << curTimer()/CLOCKS_PER_SEC << " secs" << endl;

    // Init m_timages by mutually exclusive clustering
    setTimages();
    m_oimages.resize((int)m_timages.size());
}

///
/// \brief Cbundle::run
///
/// \param prefix                   -
/// \param imageThreshold
/// \param tau
/// \param scoreRatioThreshold
/// \param coverageThreshold
/// \param pnumThreshold
/// \param CPU
///
void Cbundle::run(const std::string prefix, const int imageThreshold,
                  const int tau, const float scoreRatioThreshold,
                  const float coverageThreshold,
                  const int pnumThreshold, const int CPU) {
    startTimer();

    prep(prefix, imageThreshold, tau, scoreRatioThreshold,
         coverageThreshold, pnumThreshold, CPU);

    cerr << '\t' << curTimer()/CLOCKS_PER_SEC << " secs" << endl;

    prep2();

    // Assumed variables that must be set properly here
    cerr << "Adding images: " << endl;
    startTimer();
    // Add images
    // Repeat until all the clusters become at most m_imageThreshold.
    while (1) {
        addImagesP();

        int change = 0;
        vector<vector<int> > newtimages;
        cout << "Divide: " << flush;
        for (int i = 0; i < (int)m_timages.size(); ++i) {
            if ((int)m_timages[i].size() <= m_imageThreshold) {
                newtimages.push_back(m_timages[i]);
                continue;
            }
            else {
                cout << i << ' ';

                change = 1;
                // divide
                vector<vector<int> > vvi;
                divideImages(m_timages[i], vvi);
                for (int j = 0; j < (int)vvi.size(); ++j)
                    newtimages.push_back(vvi[j]);
            }
        }

        cout << endl;

        m_timages.swap(newtimages);
        if (change == 0)
            break;
    }
    cerr << "done\t" << curTimer()/CLOCKS_PER_SEC << " secs" << endl;

    m_oimages.resize((int)m_timages.size());

//    writeCameraCenters();

    // Output results
    writeVis();
    writeGroups();
}

float Cbundle::computeLink(const int image0, const int image1) {
    vector<int> common;
    //Intersection of functions
    set_intersection(m_vpoints[image0].begin(), m_vpoints[image0].end(),
                     m_vpoints[image1].begin(), m_vpoints[image1].end(),
                     back_inserter(common));

    float score = 0.0f;
    for (int i = 0; i < (int)common.size(); ++i) {
        const int pid = common[i];
        vector<int> vtmp;
        vtmp.push_back(image0);    vtmp.push_back(image1);
        const float ftmp = computeScore2(m_coords[pid], vtmp);
        if (m_sfms2[pid].m_score != 0.0f)
            score += m_pweights[pid] *
                    ftmp / (m_sfms2[pid].m_scoreThreshold / m_scoreRatioThreshold);
    }

    return score;
}

void Cbundle::slimNeighborsSetLinks(void) {
    const int maxneighbor = 30;
    m_links.clear();
    m_links.resize(m_cnum);

#pragma omp parallel for
    for (int image = 0; image < m_cnum; ++image) {
        m_links[image].resize((int)m_neighbors[image].size());

        for (int i = 0; i < (int)m_neighbors[image].size(); ++i)
            m_links[image][i] = computeLink(image, m_neighbors[image][i]);

        if ((int)m_neighbors[image].size() < 2)
            continue;

        vector<int> newneighbors;
        vector<float> newlinks;

        vector<Vec2f> vv;
        for (int i = 0; i < (int)m_neighbors[image].size(); ++i)
            vv.push_back(Vec2(-m_links[image][i], m_neighbors[image][i]));
        sort(vv.begin(), vv.end(), Svec2cmp<float>());

        //    cout<<"size:"<<m_neighbors[image].size()<<endl;
        //    for(int i=0;i<vv.size();i++)
        //    {
        //        cout<<"vv:"<<vv[i]<<endl;
        //    }

        const int itmp = min(maxneighbor, (int)vv.size());
        for (int i = 0; i < itmp; ++i) {
            newneighbors.push_back((int)vv[i][1]);
            newlinks.push_back(-vv[i][0]);
        }

        m_neighbors[image].swap(newneighbors);
        m_links[image].swap(newlinks);
    }
}

void Cbundle::setScoreThresholds(void) {
#pragma omp parallel for
    for (int p = 0; p < (int)m_coords.size(); ++p) {
        m_sfms2[p].m_scoreThreshold =
                computeScore2(m_coords[p], m_visibles[p], m_sfms2[p].m_uimages)
                * m_scoreRatioThreshold;

    }
}

void Cbundle::sRemoveImages(void) {
    m_removed.clear();
    m_removed.resize(m_cnum, 0);
    
    m_allows.resize(m_cnum);
    for (int c = 0; c < m_cnum; ++c)
        m_allows[c] = (int)ceil((int)m_vpoints[c].size() *
                                (1.0f - m_coverageThreshold)); //num of points in each image at least

    // Sort all the images in an increasing order of resolution
    vector<Vec2i> vvi;
    for (int c = 0; c < m_cnum; ++c) {
        const int res =
                m_pss.m_photos[c].getWidth(0) * m_pss.m_photos[c].getHeight(0);
        vvi.push_back(Vec2i(res, c));
    }
    sort(vvi.begin(), vvi.end(), Svec2cmp<int>());

    const int tenth = max(1, m_cnum / 10);
    for (int i = 0; i < (int)vvi.size(); ++i) {
        if (i % tenth == 0)
            cerr << '*' << flush;
        const int image = vvi[i][1];
        checkImage(image);
    }
    cerr << endl;
    
    cerr << "Kept: ";
    int kept = 0;
    for (int c = 0; c < m_cnum; ++c)
        if (m_removed[c] == 0) {
            ++kept;
            cerr << c << ' ';
        }
    cerr << endl << endl;

    cerr << "Removed: ";
    for (int c = 0; c < m_cnum; ++c)
        if (m_removed[c]) {
            cerr << c << ' ';
        }
    cerr << endl;
    cerr << "sRemoveImages: " << m_cnum << " -> " << kept << flush;
}

void Cbundle::resetVisibles(void) {
    // reset m_visibles. remove "removed images" from the list.
    for (int p = 0; p < (int)m_visibles.size(); ++p) {
        vector<int> newimages;

        setNewImages(p, -1, newimages);
        m_visibles[p].swap(newimages);
    }
}

void Cbundle::setNewImages(const int pid, const int rimage,
                           std::vector<int>& newimages) {
    for (int i = 0; i < (int)m_visibles[pid].size(); ++i) {
        const int image = m_visibles[pid][i];
        if (m_removed[image] || image == rimage)
            continue;
        newimages.push_back(image);
    }
}

void Cbundle::checkImage(const int image) {
    // For each SfM, check if it will be unsatisfied by removing image.
    //  0: unsatisfy
    //  1: satisfy->satisfy
    //  2: satisfy->unsatisfy
    m_statsT.resize((int)m_vpoints[image].size());
    m_jobs.clear();

#pragma omp parallel for
    for (int p = 0; p < (int)m_statsT.size(); ++p) {
        const int pid = m_vpoints[image][p];
        if (m_sfms2[pid].m_satisfied == 0)
            m_statsT[p] = 0;
        else {
            m_statsT[p] = 1;

            // if m_uimages of p is not removed, if image is not in
            // m_uimages, the point is still satisfied.
            int valid = 1;
            int inside = 0;

            for (int i = 0; i < (int)m_sfms2[pid].m_uimages.size(); ++i) {
                const int itmp = m_sfms2[pid].m_uimages[i];
                if (itmp == image)
                    inside = 1;
                if (m_removed[itmp]) {  //?????
                    cout<<"it is run!\n";
                    valid = 0;
                    break;
                }
            }

            if (valid == 1 && inside == 0)
                continue;

            vector<int> newimages;
            setNewImages(pid, image, newimages);
            const float cscore = computeScore2(m_coords[pid], newimages);
            if (cscore < m_sfms2[pid].m_scoreThreshold)//?????
                m_statsT[p] = 2;
        }
    }

    // For each image, how many SFM points are removed if you remove
    // "image"
    vector<int> decrements;
    decrements.resize(m_cnum, 0);

    // Look at points with m_stastT[p] = 2, to see if m_allows are still
    // above thresholds.
    for (int p = 0; p < (int)m_statsT.size(); ++p) {
        if (m_statsT[p] == 2) {
            const int pid = m_vpoints[image][p];
            for (int i = 0; i < (int)m_visibles[pid].size(); ++i) {
                const int itmp = m_visibles[pid][i];
//                cout<<decrements[itmp]<<endl;
                ++decrements[itmp];
            }
        }
    }

    // If m_allows can cover decrements, go ahead.
    int rflag = 1;
    for (int c = 0; c < m_cnum; ++c)
        if (m_allows[c] < decrements[c]) {
            rflag = 0;
            break;
        }

    // remove
    if (rflag) {
        m_removed[image] = 1;
        for (int p = 0; p < (int)m_statsT.size(); ++p)
            if (m_statsT[p] == 2)
                m_sfms2[m_vpoints[image][p]].m_satisfied = 0;

        for (int c = 0; c < m_cnum; ++c)
            m_allows[c] -= decrements[c];

        // Update uimages for points that are still satisfied and still
        // contains image in m_uimages
#pragma omp parallel for
        for (int p = 0; p < (int)m_statsT.size(); ++p) {
            const int pid = m_vpoints[image][p];
            if (m_statsT[p] == 1) {
                int contain = 0;
                for (int i = 0; i < (int)m_sfms2[pid].m_uimages.size(); ++i)
                    if (m_sfms2[pid].m_uimages[i] == image) {
                        contain = 1;
                        break;
                    }

                if (contain) {
                    vector<int> newimages;
                    setNewImages(pid, -1, newimages);
                    const float cscore =
                            computeScore2(m_coords[pid], newimages, m_sfms2[pid].m_uimages);
                    if (cscore < m_sfms2[pid].m_scoreThreshold)
                        m_sfms2[pid].m_satisfied = 0;
                }
            }
        }
    }
}

void Cbundle::setNeighbors(void) {
    m_neighbors.clear();
    m_neighbors.resize(m_cnum);
#pragma omp parallel for
    for (int image = 0; image < m_cnum; ++image) {
        vector<int> narray;
        narray.resize(m_cnum, 0);

        for (int p = 0; p < (int)m_visibles.size(); ++p) {
            if (!binary_search(m_visibles[p].begin(), m_visibles[p].end(),
                               image))
                continue;
            for (int i = 0; i < (int)m_visibles[p].size(); ++i)
                narray[m_visibles[p][i]] = 1;
        }

        for (int i = 0; i < m_cnum; ++i)
            if (narray[i] && i != image)
                m_neighbors[image].push_back(i);
    }
}

void Cbundle::setTimages(void) {
    vector<int> lhs;
    for (int c = 0; c < m_cnum; ++c)
        if (m_removed[c] == 0)
            lhs.push_back(c);

    m_timages.clear();
    cout<<m_timages.size()<<endl;

    if ((int)lhs.size() <= m_imageThreshold)
        m_timages.push_back(lhs);
    else
        divideImages(lhs, m_timages);//??????

    cerr << endl << "Cluster sizes: " << endl;
    cout<<m_timages.size()<<endl;
    for (int i = 0; i < (int)m_timages.size(); ++i)
        cerr << (int)m_timages[i].size() << ' ';
    cerr << endl;
}

void Cbundle::divideImages(const std::vector<int>& lhs,
                           std::vector<std::vector<int> >& rhs) {
    const float iratio = 125.0f / 150.0f; //?????
    // Candidates
    list<vector<int> > candidates;
    // initialize first cluster
    candidates.push_back(vector<int>());
    cout<<"candidate:"<<candidates.size()<<endl;
    vector<int>& vitmp = candidates.back();
    for (int i = 0; i < (int)lhs.size(); ++i)
        vitmp.push_back(lhs[i]);

    // Iterate
    while (1) {
        if (candidates.empty())
            break;
        vector<int> cand = candidates.front();
        candidates.pop_front();
        if ((int)cand.size() <= m_imageThreshold * iratio) {
            rhs.push_back(cand);
            continue;
        }
        // Divide into 2 groups
        vector<idxtype> xadj, adjncy, adjwgt, part;
        const int nparts = 2;
        const int cutType = 0;
        // Define graphs
        for (int i = 0; i < (int)cand.size(); ++i) {
            xadj.push_back((int)adjncy.size());

            for (int j = 0; j < (int)cand.size(); ++j) {
                if (i == j)
                    continue;
                // Check if cand[i] and cand[j] are connected
                const int cid0 = cand[i];
                const int cid1 = cand[j];
                vector<int>::const_iterator nite =
                        find(m_neighbors[cid0].begin(), m_neighbors[cid0].end(), cid1);

                if (nite != m_neighbors[cid0].end()) {
                    adjncy.push_back(j);
                    const int offset = nite - m_neighbors[cid0].begin();
                    const int weight =
                            min(5000, (int)floor(10.0f * m_links[cid0][offset] + 0.5f));
                    adjwgt.push_back(weight);
                }
            }
        }
        xadj.push_back((int)adjncy.size());

        Cgraclus::runE(xadj, adjncy, adjwgt, nparts, cutType, part);

        // Divide into 2 groups
        vector<int> cand1, cand2;
        for (int i = 0; i < (int)part.size(); ++i) {
            if (part[i] == 0)
                cand1.push_back(cand[i]);
            else
                cand2.push_back(cand[i]);
        }

        if (cand1.empty() || cand2.empty()) {
            cerr << "Error. Normalized cuts produced an empty cluster: "
                 << (int)part.size() << " -> "
                 << (int)cand1.size() << ' '
                 << (int)cand2.size() << endl;
            exit (1);
        }

        if ((int)cand1.size() <= m_imageThreshold * iratio)
            rhs.push_back(cand1);
        else {
            candidates.push_back(vector<int>());
            (candidates.back()).swap(cand1);
        }
        if ((int)cand2.size() <= m_imageThreshold * iratio)
            rhs.push_back(cand2);
        else {
            candidates.push_back(vector<int>());
            (candidates.back()).swap(cand2);
        }
    }
}

void Cbundle::readBundle(const std::string file) {
    // For each valid image, a list of connected images, and the second
    // value is the number of common points.
    ifstream ifstr;  ifstr.open(file.c_str());
    if (!ifstr.is_open()) {
        cerr << "Bundle file not found: " << file << endl;
        exit (1);
    }
    while (1) {
        unsigned char uctmp;
        ifstr.read((char*)&uctmp, sizeof(unsigned char));
        ifstr.putback(uctmp);
        if (uctmp == '#') {
            char buffer[1024];      ifstr.getline(buffer, 1024);
        }
        else
            break;
    }

    //cnum:camera_num,p_num:point_num,id:picture_id
    int cnum, pnum;
    ifstr >> cnum >> pnum;
    vector<int> ids;        ids.resize(cnum);
    cerr << cnum << " cameras -- " << pnum << " points in bundle file" << endl;
    m_cnum = 0;
    for (int c = 0; c < cnum; ++c) {
        ids[c] = -1;
        //camera_params:camera_focal length,k1,k2,9 rotations,3 translations
        float params[15];
        for (int i = 0; i < 15; ++i)
            ifstr >> params[i];
        if (params[0] != 0.0f)
            ids[c] = m_cnum++;
    }

    m_vpoints.resize(m_cnum);

    m_coords.clear();  m_visibles.clear();
    m_colors.clear();
    m_pnum = 0;

    const int tenth = max(1, pnum / 10);
    for (int p = 0; p < pnum; ++p) {
        if (p % tenth == 0)
            cerr << '*' << flush;
        int num;    Vec3f color;
        //world points,num is the bumbers of visible points,coord is 3D Points
        Vec4f coord;
        ifstr >> coord[0] >> coord[1] >> coord[2]
                >> color >> num;
        coord[3] = 1.0f;
        //    cout<<"3D Point:"<<coord[0]<<" "<<coord[1]<<" "<<coord[2]<<endl;
        //    cout<<"num:"<<num<<endl;

        /// a set of picture id where the point can be seen
        /// visibles
        vector<int> visibles;
        for (int i = 0; i < num; ++i) {
            int itmp;      ifstr >> itmp;
            //      cout<<"itmp:"<<itmp<<endl;
            if (cnum <= itmp) {
                double dtmp;
                ifstr >> itmp >> dtmp >> dtmp;//is not visible Points
                continue;
            }
            if (ids[itmp] == -1) {
                cerr << "impossible " << itmp << ' ' << ids[itmp] << endl;
                exit (1);
            }
            //      cout<<itmp<<"   ids[itmp]:"<<ids[itmp]<<endl;
            visibles.push_back(ids[itmp]);

            // Based on the bundler version, the number of parameters here
            // are either 1 or 3. Currently, it seems to be 3.
            double dtmp;
            ifstr >> itmp >> dtmp >> dtmp;
            //      cout<<"Point:"<<itmp<<" "<<dtmp<<" "<<dtmp<<endl;
            //ifstr >> dtmp;
        }
        if ((int)visibles.size() < 2)
            continue;

        //          cout<<"size:"<visibles.size()<<endl;

        //    for(vector<int>::iterator it=visibles.begin(); it!=visibles.end(); it++)
        //    {
        //        cout<<*it<<endl;
        //    }
        sort(visibles.begin(), visibles.end());

        for (int i = 0; i < (int)visibles.size(); ++i) {
            m_vpoints[visibles[i]].push_back(m_pnum);
        }

        m_visibles.push_back(visibles);


        m_coords.push_back(coord);

        m_colors.push_back(color);
        ++m_pnum;
    }
    //  cout<<m_coords.size()<<endl;
    //  cout<<m_visibles.size()<<endl;
    ifstr.close();
    setNeighbors();

    cerr << endl << m_cnum << " cameras -- " << m_pnum << " points" << flush;
}

void Cbundle::findPNeighbors(sfcnn<const float*, 3, float>& tree,
                             const int pid, std::vector<int>& pneighbors) {
    const float thresh = m_dscale2 * m_dscale2 *
            m_minScales[pid] * m_minScales[pid];

    vector<long unsigned int> ids;
    vector<double> dists;
    int k = min((int)m_coords.size(), 400);

    while (1) {
        ids.clear();
        dists.clear();
        tree.ksearch(&m_coords[pid][0], k, ids, dists);


        if (thresh < dists[k - 1])
            break;
        if (k == (int)m_coords.size())
            break;

        k = min((int)m_coords.size(), k * 2);

    }

//    cout<<k<<endl;
    for (int i = 0; i < k; ++i) {
        if (thresh < dists[i])
            break;

        // If distance from pid2 is also within threshold ok
        const float thresh2 = m_dscale2 * m_dscale2 *
                m_minScales[ids[i]] * m_minScales[ids[i]];
        if (thresh2 < dists[i])
            continue;

        if (ids[i] != (unsigned int)pid)
            pneighbors.push_back(ids[i]);
    }
}

void Cbundle::mergeSfMPThread(void) {
    const int tenth = (int)m_coords.size() / 10;
    while (1) {
        int pid = -1;
        mtx_lock(&m_lock);
        if (!m_jobs.empty()) {
            pid = m_jobs.front();
            m_jobs.pop_front();
        }
        if (pid != -1 && m_merged[pid])
            pid = -2;
        if (m_count % tenth == 0)
            cerr << '*' << flush;
        ++m_count;
        mtx_unlock(&m_lock);
        if (pid == -2)
            continue;
        if (pid == -1)
            break;

        // Process, and check later if merged flag is on
        vector<int> pneighbors;
        findPNeighbors(*m_ptree, pid, pneighbors);
        const int psize = (int)pneighbors.size();
//        cout<<psize<<endl;
        // visible images and its neighbors
        vector<int> visibles = m_visibles[pid];
        vector<int> vitmp;
        for (int i = 0; i < (int)m_visibles[pid].size(); ++i) {
            const int imagetmp = m_visibles[pid][i];
            vitmp.clear();
            mymerge(visibles, m_neighbors[imagetmp], vitmp);//
            vitmp.swap(visibles);
        }

        vector<char> visflag(psize, 0);
        // test visibility
        for (int i = 0; i < psize; ++i) {
            const int& pid2 = pneighbors[i];
            if (my_isIntersect(visibles, m_visibles[pid2]))
                visflag[i] = 1;
        }

        // Now lock and try to register
        mtx_lock(&m_lock);
        // If the main one is removed, over... waste.

        if (m_merged[pid] == 0) {

            m_merged[pid] = 1;
            for (int i = 0; i < psize; ++i) {
                const int pid2 = pneighbors[i];
                if (visflag[i] && m_merged[pid2] == 0) {
                    m_merged[pid2] = 1;
                    m_puf->union_set(pid, pid2);//????????
                }
            }
        }
        mtx_unlock(&m_lock);
    }
}

int Cbundle::mergeSfMPThreadTmp(void* arg) {
    ((Cbundle*)arg)->mergeSfMPThread();
    return 0;
}

void Cbundle::mergeSfMP(void) {
    // Repeat certain times until no change
    const int cpnum = (int)m_coords.size();
    m_minScales.resize(cpnum);
    //get min scale for each point in visible image
    for (int p = 0; p < cpnum; ++p) {
        m_minScales[p] = INT_MAX/2;
        for (int i = 0; i < (int)m_visibles[p].size(); ++i) {
            const int image = m_visibles[p][i];
            const float stmp =
                    m_pss.m_photos[image].getScale(m_coords[p], m_levels[image]);
            m_minScales[p] = min(m_minScales[p], stmp);
        }
    }

    m_puf = new disjoint_sets_with_storage<>(cpnum);

    for (int p = 0; p < cpnum; ++p)
    {
        //        cout<<cpnum<<endl;
        m_puf->make_set(p);
    }

    {
        // kdtree
        vector<const float*> ppoints;
        ppoints.resize((int)m_coords.size());
        cout<<endl<<"size:"<<m_coords.size()<<endl;
        for (int i = 0; i < (int)m_coords.size(); ++i)
        {
//            cout<<m_coords[i][0]<<endl;
            ppoints[i] = &(m_coords[i][0]); //get m_coords's x position
        }
        cout<<ppoints.size()<<endl;
        m_ptree =new sfcnn<const float*, 3, float>(&ppoints[0], (int)ppoints.size());//creat 3-Dim tree

        m_merged.resize((int)m_coords.size(), 0);
//        for(int i=0;i<m_merged.size();i++)
//            cout<<endl<<"size:"<<(double)m_merged[i]<<endl;
        m_jobs.clear();
        vector<int> order;
        order.resize(cpnum);
        cout<<order.size()<<endl;
        for (int p = 0; p < cpnum; ++p)
            order[p] = p;
        random_shuffle(order.begin(), order.end());  //Random Rearrange

        for (int p = 0; p < cpnum; ++p)
            m_jobs.push_back(order[p]);

        m_count = 0;
        vector<thrd_t> threads(m_CPU);
        for (int c = 0; c < m_CPU; ++c)
            thrd_create(&threads[c], &mergeSfMPThreadTmp, (void*)this);
        for (int c = 0; c < m_CPU; ++c)
            thrd_join(threads[c], NULL);

        cout<<endl<<"size:"<<m_coords.size()<<endl;
        int newpnum = 0;
        // Mapping from m_pnum to new id for reps
        vector<int> dict;
        vector<int> reps;
        dict.resize((int)m_coords.size(), -1);

        for (int p = 0; p < (int)m_coords.size(); ++p) {
//            cout<<"m_coords:"<<m_coords[p]<<endl;
            const int ptmp = m_puf->find_set(p);//????
//            cout<<ptmp<<endl;
            if (p == ptmp) {
                dict[p] = newpnum;
                reps.push_back(p);
                ++newpnum;
            }
        }
        cout<<newpnum<<endl;
    }

    // Based on m_puf, reset m_coords, m_coords, m_visibles, m_vpoints
    cerr << "resetPoints..." << flush;
    resetPoints();
    cerr << "done" << endl;

    delete m_puf;
    m_puf = NULL;
    delete m_ptree;
    m_ptree = NULL;

    const int npnum = (int)m_coords.size();
    cerr << "Rep counts: " << cpnum << " -> " << npnum << "  " << flush;
}

// Based on m_puf, reset m_coords, m_coords, m_visibles, m_vpoints
void Cbundle::resetPoints(void) {
    vector<int> counts;
    vector<int> smallestids;
    counts.resize((int)m_coords.size(), 0);
    smallestids.resize((int)m_coords.size(), INT_MAX/2);
    cout<<m_coords.size()<<endl;
    for (int p = 0; p < (int)m_coords.size(); ++p) {
        const int ptmp = m_puf->find_set(p);//?????????
        //    cout<<"ptmp:"<<ptmp<<endl;
        smallestids[ptmp] = min(smallestids[ptmp], p);
        ++counts[ptmp];
        //    cout<<counts[ptmp]<<endl;
    }
    const int mthreshold = 2;
    vector<Vec2i> vv2;
    for (int p = 0; p < (int)m_coords.size(); ++p)
        if (mthreshold <= counts[p])
            vv2.push_back(Vec2i(smallestids[p], p));
    sort(vv2.begin(), vv2.end(), Svec2cmp<int>());
    //  cout<<"vv2 size:"<<vv2.size()<<endl;
    vector<int> newpids;
    newpids.resize((int)m_coords.size(), -1);
    int newpnum = (int)vv2.size();
    for (int i = 0; i < newpnum; ++i)
        newpids[vv2[i][1]] = i;

    vector<Vec4f> newcoords;  newcoords.resize(newpnum);
    vector<vector<int> > newvisibles;  newvisibles.resize(newpnum);

    for (int p = 0; p < (int)m_coords.size(); ++p) {
        const int ptmp = m_puf->find_set(p);
        if (counts[ptmp] < mthreshold)
            continue;

        const int newpid = newpids[ptmp];
        newcoords[newpid] += m_coords[p];
        vector<int> vitmp;
        mymerge(newvisibles[newpid], m_visibles[p], vitmp);
        vitmp.swap(newvisibles[newpid]);
    }

    for (int i = 0; i < newpnum; ++i)
        newcoords[i] = newcoords[i] / newcoords[i][3];

    // Update m_vpoints
    m_coords.swap(newcoords);
    m_visibles.swap(newvisibles);
      cout<<"new size:"<<m_coords.size()<<endl;
    for (int c = 0; c < m_cnum; ++c)
        m_vpoints[c].clear();

    for (int p = 0; p < (int)m_coords.size(); ++p) {
        for (int i = 0; i < (int)m_visibles[p].size(); ++i) {
            const int itmp = m_visibles[p][i];
            m_vpoints[itmp].push_back(p);
        }
    }

    m_pweights.clear();
    for (int i = 0; i < newpnum; ++i)
        m_pweights.push_back(counts[vv2[i][1]]);
}

void Cbundle::setScoresClusters(void) {
#pragma omp parallel for
        cout<<m_coords.size()<<endl;
    //    cout<<m_cnum<<endl;
    for (int p = 0; p < (int)m_coords.size(); ++p) {
        // if m_satisfied is 0, no hope (even all the images are in the
        // cluster, not satisfied
        if (m_sfms2[p].m_satisfied == 0)
            continue;

        m_sfms2[p].m_satisfied = 2;
        setCluster(p);
    }
}

void Cbundle::setCluster(const int p) {
    m_sfms2[p].m_score = -1.0f;
    m_sfms2[p].m_cluster = -1;
    for (int c = 0; c < (int)m_timages.size(); ++c) {
        // Select images in cluster
        vector<int> vitmp;
        set_intersection(m_timbingages[c].begin(), m_timages[c].end(),
                         m_visibles[p].begin(), m_visibles[p].end(),
                         back_inserter(vitmp));

        const float stmp = computeScore2(m_coords[p], vitmp);
        if (m_sfms2[p].m_score < stmp) {
            m_sfms2[p].m_cluster = c;
            m_sfms2[p].m_score = stmp;
        }
    }

    // If no cluster contains 2 images
    if (m_sfms2[p].m_cluster == -1) {
        cout<<"it is cluster!\n";
        int find = 0;
        for (int j = 0; j < (int)m_visibles[p].size(); ++j) {
            if (find)
                break;
            for (int c = 0; c < (int)m_timages.size(); ++c)
                if (binary_search(m_timages[c].begin(), m_timages[c].end(),
                                  m_visibles[p][j])) {
                    m_sfms2[p].m_cluster = c;
                    m_sfms2[p].m_score = 0.0f;
                    find = 1;
                    break;
                }
        }
        // If none of the visibles images are
        if (find == 0) {
            cerr << "Impossible in setscoresclustersthread" << endl
                 << (int)m_visibles[p].size() << endl
                 << (int)m_sfms2[p].m_satisfied << endl;
            exit (1);
        }
    }

    if (m_sfmdangs2[p].m_scoreThreshold <= m_sfms2[p].m_score) {
        // SATISFIED
        m_sfms2[p].m_satisfied = 1;

        //  update m_lacks
        mtx_lock(&m_lock);
        for (int i = 0; i < (int)m_visibles[p].size(); ++i) {
            const int image = m_visibles[p][i];
            --m_lacks[image];
        }
        mtx_unlock(&m_lock);
    }
}

float Cbundle:: angleScore(const Vec4f& ray0, const Vec4f& ray1) {
    //    cout<<"ray[0]:"<<ray0<<" "<<ray1<<endl;
    //Gaussian model????
    const static float lsigma = 5.0f * M_PI / 180.f;//?
    const static float rsigma = 15.0f * M_PI / 180.0f;//?
    const static float lsigma2 = 2.0f * lsigma * lsigma;//?
    const static float rsigma2 = 2.0f * rsigma * rsigma;//?
    const static float pivot = 20.0f * M_PI / 180.0f;//?

    const float angle = acos(min(1.0f, ray0 * ray1));//arcos
    const float diff = angle - pivot;

    if (angle < pivot)
        return exp(- diff * diff / lsigma2);
    else
        return exp(- diff * diff / rsigma2);
}

void Cbundle::setClusters(void) {
#pragma omp parallel for
    for (int p = 0; p < (int)m_sfms2.size(); ++p) {
        if (m_sfms2[p].m_satisfied != 2)
            continue;

        setCluster(p);
    }
}

void Cbundle::addImagesP(void) {
    // set m_lacks (how many more sfm points need to be satisfied)
    m_lacks.resize(m_cnum);
    for (int c = 0; c < m_cnum; ++c) {
        if (m_removed[c])
            m_lacks[c] = 0;
        else
            m_lacks[c] =
                    (int)floor((int)m_vpoints[c].size() * m_coverageThreshold);
    }

    // Compute best score given all the images. Upper bound on the score
    setScoresClusters();

    // Add images to clusters to make m_lacks at most 0 for all the
    // images. In practice, for each cluster, identify corresponding sfm
    // points that have not been satisfied. These points compute gains.

    // set m_adds for each sfm point, and add images

    m_addnums.clear();
    m_addnums.resize((int)m_timages.size(), 0);

    while (1) {
        const int totalnum = addImages();
        // Very end
        if (totalnum == 0) {
            break;
        }
        // If one of the cluster gets more than m_imageThreshold, divide
        int divide = 0;
        for (int i = 0; i < (int)m_timages.size(); ++i)
            if (m_imageThreshold < (int)m_timages[i].size()) {
                divide = 1;
                break;
            }
        if (divide) {
            break;
        }

        setClusters();
    }

    for (int i = 0; i < (int)m_addnums.size(); ++i)
        cerr << m_addnums[i] << ' ';
    cerr << endl;

    int totalnum = 0;
    for (int c = 0; c < (int)m_timages.size(); ++c)
        totalnum += (int)m_timages[c].size();
    int beforenum = 0;
    for (int c = 0; c < m_cnum; ++c)
        if (m_removed[c] == 0)
            ++beforenum;

    cerr << "Image nums: "
         << m_cnum << " -> " << beforenum <<  " -> " << totalnum << endl;
}

int Cbundle::addImages(void) {
    m_thread = 0;
    m_jobs.clear();

    // we think about sfm points that belong to images that have not been satisfied
    vector<char> flags;
    flags.resize((int)m_coords.size(), 0);
    for (int c = 0; c < m_cnum; ++c) {
        if (m_lacks[c] <= 0)
            continue;
        else {
            for (int i = 0; i < (int)m_vpoints[c].size(); ++i) {
                const int pid = m_vpoints[c][i];
                if (m_sfms2[pid].m_satisfied == 2)
                    flags[pid] = 1;
            }
        }
    }

#pragma omp parallel for
    for (int p = 0; p < (int)m_sfms2.size(); ++p) {
        if (flags[p] == 0)
            continue;
        m_sfms2[p].m_adds.clear();

        const int cluster = m_sfms2[p].m_cluster;
        // Try to add an image to m_timages[cluster]
        vector<int> cimages;
        set_intersection(m_timages[cluster].begin(), m_timages[cluster].end(),
                         m_visibles[p].begin(), m_visibles[p].end(),
                         back_inserter(cimages));
        sort(cimages.begin(), cimages.end());

        for (int i = 0; i < (int)m_visibles[p].size(); ++i) {
            const int image = m_visibles[p][i];

            if (binary_search(cimages.begin(), cimages.end(), image))
                continue;

            vector<int> vitmp = cimages;
            vitmp.push_back(image);
            const float newscore = computeScore2(m_coords[p], vitmp);
            if (newscore <= m_sfms2[p].m_score)
                continue;

            m_sfms2[p].m_adds.push_back(Sadd(image,
                                             (newscore - m_sfms2[p].m_score) /
                                             m_sfms2[p].m_scoreThreshold));
        }
    }

    // Accumulate information from SfM points. For each cluster.
    // For each cluster, for each image, sum of gains.
    vector<map<int, float> > cands;
    cands.resize((int)m_timages.size());

    for (int p = 0; p < (int)m_sfms2.size(); ++p) {
        if (flags[p] == 0)
            continue;

        const int cluster = m_sfms2[p].m_cluster;

        for (int i = 0; i < (int)m_sfms2[p].m_adds.size(); ++i) {
            Sadd& stmp = m_sfms2[p].m_adds[i];
            cands[cluster][stmp.m_image] += stmp.m_gain;
        }
    }

    return addImagesSub(cands);
}

int Cbundle::addImagesSub(const std::vector<std::map<int, float> >& cands) {
    // Vec3f (gain, cluster, image). Start adding starting from the good
    // one, block neighboring images.
    vector<Vec3f> cands2;
    for (int i = 0; i < (int)m_timages.size(); ++i) {
        map<int, float>::const_iterator mbegin = cands[i].begin();
        map<int, float>::const_iterator mend = cands[i].end();

        while (mbegin != mend) {
            cands2.push_back(Vec3f(-mbegin->second, i, mbegin->first));
            ++mbegin;
        }
    }

    if (cands2.empty())
        return 0;

    sort(cands2.begin(), cands2.end(), Svec3cmp<float>());

    vector<char> blocked;
    blocked.resize(m_cnum, 0);
    vector<int> addnum;
    addnum.resize((int)m_timages.size(), 0);

    // A bit of tuning is possible here. For the paper, we used 0.7f,
    //but 0.9f seems to produce better results in general.  const float
    //gainThreshold = -cands2[0][0] * 0.7f;
    const float gainThreshold = -cands2[0][0] * 0.9f;

    // use rato threshold for blocked one. if not blocked, just keep on
    // going.
    for (int i = 0; i < (int)cands2.size(); ++i) {
        const float gain = -cands2[i][0];
        if (gain < gainThreshold)
            break;

        const int cluster = (int)cands2[i][1];
        const int image = (int)cands2[i][2];

        if (blocked[image])
            continue;

        // Add
        ++addnum[cluster];
        ++m_addnums[cluster];
        blocked[image] = 1;
        for (int i = 0; i < (int)m_neighbors[image].size(); ++i)
            blocked[m_neighbors[image][i]] = 1;

        m_timages[cluster].push_back(image);
        // m_score, m_cluster, m_satisfied, m_lacks are updated in
        // setClusters.  So, no need to update anything.
    }

    for (int i = 0; i < (int)m_timages.size(); ++i)
        sort(m_timages[i].begin(), m_timages[i].end());

    return accumulate(addnum.begin(), addnum.end(), 0);
}

int Cbundle::totalNum(void) const{
    int totalnum = 0;
    for (int c = 0; c < (int)m_timages.size(); ++c)
        totalnum += (int)m_timages[c].size();
    return totalnum;
}

int Cbundle::my_isIntersect(const std::vector<int>& lhs,
                            const std::vector<int>& rhs) {
    vector<int>::const_iterator b0 = lhs.begin();
    vector<int>::const_iterator e0 = lhs.end();

    vector<int>::const_iterator b1 = rhs.begin();
    vector<int>::const_iterator e1 = rhs.end();

    while (1) {
        if (b0 == e0)
            return 0;
        if (b1 == e1)
            return 0;

        if (*b0 == *b1) {
            return 1;
        }
        else if (*b0 < *b1)
            ++b0;
        else
            ++b1;
    }
}

void Cbundle::mymerge(const std::vector<int>& lhs,
                      const std::vector<int>& rhs,
                      std::vector<int>& output) {
    vector<int>::const_iterator b0 = lhs.begin();
    vector<int>::const_iterator e0 = lhs.end();

    vector<int>::const_iterator b1 = rhs.begin();
    vector<int>::const_iterator e1 = rhs.end();

    while (1) {
        if (b0 == e0) {
            output.insert(output.end(), b1, e1);
            break;
        }
        if (b1 == e1) {
            output.insert(output.end(), b0, e0);
            break;
        }

        if (*b0 == *b1) {
            output.push_back(*b0);
            ++b0;
            ++b1;
        }
        else if (*b0 < *b1) {
            output.push_back(*b0);
            ++b0;
        }
        else {
            output.push_back(*b1);
            ++b1;
        }
    }
}

// Calculates log2 of number.
template <typename T>
T Log2( T n )
{
    // log(n)/log(2) is log2.
    return log( n ) / log( T(2) );
}

void Cbundle::setWidthsHeightsLevels(void) {
    m_widths.resize(m_cnum);
    m_heights.resize(m_cnum);
    m_levels.resize(m_cnum);

    // Determine level. SfM was done on 2M pixels.
    for (int c = 0; c < m_cnum; ++c) {
        m_levels[c] = m_dlevel;

        m_widths[c] = m_pss.m_photos[c].getWidth(m_levels[c]);
        m_heights[c] = m_pss.m_photos[c].getHeight(m_levels[c]);
    }
}


float Cbundle::computeScore2(const Vec4f& coord,
                             const std::vector<int>& images) const {
    vector<int> uimages;
    return computeScore2(coord, images, uimages);
}

float Cbundle::computeScore2(const Vec4f& coord,
                             const std::vector<int>& images,
                             std::vector<int>& uimages) const {
    const int inum = (int)images.size();
    uimages.clear();
    if (inum < 2)
        return -1.0f;

    vector<Vec4f> rays;    rays.resize(inum);
    vector<float> scales;  scales.resize(inum);

    //compute rays
    for (int r = 0; r < inum; ++r) {
        rays[r] = m_pss.m_photos[images[r]].m_center - coord;
        //   cout<<"ray[r]:"<<m_pss.m_photos[images[r]].m_center<<endl;
        unitize(rays[r]);
        //    cout<<"ray[r]:"<<rays[r]<<endl;

        scales[r] = 1.0f / m_pss.m_photos[images[r]].getScale(coord, 0);
    }

    // Find the best pair of images
    Vec2i bestpair;
    float bestscore = -INT_MAX/2;
    for (int i = 0; i < inum; ++i)
        for (int j = i+1; j < inum; ++j) {
            const float ftmp =
                    angleScore(rays[i], rays[j]) * scales[i] * scales[j];
            if (bestscore < ftmp) {
                //        cout<<"bestPair:"<<bestscore<<" "<<inum<<" "<<i<<" "<<j<<endl;
                bestscore = ftmp;
                bestpair = Vec2i(i, j);
                //        cout<<"bestPair:"<<bestpair<<endl;

            }
        }

    vector<int> inout;  inout.resize(inum, 1);
    //  cout<<"bestPair:"<<bestpair[0]<<" "<<bestpair[1]<<endl;
    inout[bestpair[0]] = 0;         inout[bestpair[1]] = 0;
    uimages.push_back(bestpair[0]);   uimages.push_back(bestpair[1]);

    for (int t = 2; t < min(m_tau, inum); ++t) {
        // Add the best new image
        int ansid = -1;
        float ansscore = -INT_MAX/2;
        for (int j = 0; j < inum; ++j) {
            if (inout[j] == 0)
                continue;

            float score = 0.0f;
            //      cout<<uimages.size()<<endl;
            for (int k = 0; k < (int)uimages.size(); ++k) {
                const int iid = uimages[k];
                score += angleScore(rays[j], rays[iid]) * scales[j] * scales[iid];
            }
            if (ansscore < score) {
                ansscore = score;
                ansid = j;
            }
        }

        if (ansid == -1) {
            cerr << "Impossible 2 in compureScore" << endl;      exit (1);
        }

        inout[ansid] = 0;
        uimages.push_back(ansid);
        bestscore += ansscore;
    }

    for (int i = 0; i < (int)uimages.size(); ++i)
        uimages[i] = images[uimages[i]];

    return bestscore;
}

float Cbundle::computeScore2(const Vec4f& coord,
                             const std::vector<int>& images,
                             const int index) const {

    vector<int> uimages;
    const int inum = (int)images.size();
    if (inum < 2)
        return -1.0f;

    vector<Vec4f> rays;    rays.resize(inum);
    //  cout<<"size:"<<rays.size()<<endl;
    vector<float> scales;  scales.resize(inum);

    for (int r = 0; r < inum; ++r) {
        rays[r] = m_pss.m_photos[images[r]].m_center - coord;
        //    cout<<"ray:"<<rays[r]<<endl;
        unitize(rays[r]);

        scales[r] = 1.0f / m_pss.m_photos[images[r]].getScale(coord, 0);
    }

    float bestscore = 0.0f;
    vector<int> inout;  inout.resize(inum, 1);
    inout[index] = 0;
    uimages.push_back(index);

    for (int t = 1; t < min(m_tau, inum); ++t) {
        // Add the best new image
        int ansid = -1;
        float ansscore = -INT_MAX/2;

        for (int j = 0; j < inum; ++j) {
            if (inout[j] == 0)
                continue;

            float score = 0.0f;
            for (int k = 0; k < (int)uimages.size(); ++k) {
                const int iid = uimages[k];
                score += angleScore(rays[j], rays[iid]) * scales[j] * scales[iid];
            }
            if (ansscore < score) {
                ansscore = score;
                ansid = j;
            }
        }

        if (ansid == -1) {
            cerr << "Impossible 2 in compureScore" << endl;      exit (1);
        }

        inout[ansid] = 0;
        uimages.push_back(ansid);
        bestscore += ansscore;
    }
    return bestscore;
}

void Cbundle::writeVis(void) {
//    ofstream ofstr;
//    char buffer[1024];
//    sprintf(buffer, "%svis.dat", m_prefix.c_str());

//    ofstr.open(buffer);
//    ofstr << "VISDATA" << endl;
//    ofstr << m_cnum << endl;

//    int numer = 0;
//    int denom = 0;

//    for (int c = 0; c < m_cnum; ++c) {
//        if (m_removed[c])
//            ofstr << c << ' ' << 0 << endl;
//        else {
//            ofstr << c << ' ' << (int)m_neighbors[c].size() << "  ";
//            for (int i = 0; i < (int)m_neighbors[c].size(); ++i)
//                ofstr << m_neighbors[c][i] << ' ';
//            ofstr << endl;

//            numer += (int)m_neighbors[c].size();
//            ++denom;
//        }
//    }
//    ofstr.close();

    m_vis.resize(m_cnum);

    for (int c = 0; c < m_cnum; ++c) {
        if (m_removed[c])
            m_vis[c].push_back(0);
//            ofstr << c << ' ' << 0 << endl;
        else {
            m_vis[c].push_back((int)m_neighbors[c].size());
//            ofstr << c << ' ' << (int)m_neighbors[c].size() << "  ";
            for (int i = 0; i < (int)m_neighbors[c].size(); ++i)
                m_vis[c].push_back(m_neighbors[c][i]);
        }
    }


//    cerr << numer / (float)denom << " images in vis on the average" << endl;

}

//void Cbundle::writeCameraCenters(void) {
//    for (int i = 0; i < (int)m_timages.size(); ++i) {
//        char buffer[1024];
//        sprintf(buffer, "%scenters-%04d.ply", m_prefix.c_str(), i);

//        ofstream ofstr;
//        ofstr.open(buffer);
//        ofstr << "ply" << endl
//              << "format ascii 1.0" << endl
//              << "element vertex " << (int)m_timages[i].size() << endl
//              << "property float x" << endl
//              << "property float y" << endl
//              << "property float z" << endl
//              << "end_header" << endl;
//        for (int j = 0; j < (int)m_timages[i].size(); ++j) {
//            const int c = m_timages[i][j];
//            ofstr << m_pss.m_photos[c].m_center[0] << ' '
//                                                   << m_pss.m_photos[c].m_center[1] << ' '
//                                                   << m_pss.m_photos[c].m_center[2] << endl;
//        }
//        ofstr.close();
//    }

//    {
//        char buffer[1024];
//        sprintf(buffer, "%scenters-all.ply", m_prefix.c_str());
//        ofstream ofstr;
//        ofstr.open(buffer);

//        ofstr << "ply" << endl
//              << "format ascii 1.0" << endl
//              << "element vertex " << m_cnum << endl
//              << "property float x" << endl
//              << "property float y" << endl
//              << "property float z" << endl
//              << "property uchar diffuse_red" << endl
//              << "property uchar diffuse_green" << endl
//              << "property uchar diffuse_blue" << endl
//              << "end_header" << endl;
//        for (int c = 0; c < m_cnum; ++c) {
//            ofstr << m_pss.m_photos[c].m_center[0] << ' '
//                                                   << m_pss.m_photos[c].m_center[1] << ' '
//                                                   << m_pss.m_photos[c].m_center[2] << ' ';

//            if (m_removed[c])
//                ofstr << "0 255 0" << endl;
//            else
//                ofstr << "255 0 255" << endl;
//        }
//        ofstr.close();
//    }
//}

void Cbundle::writeGroups(void) {
//    for (int c = 0; c < (int)m_timages.size(); ++c) {
//        for (int i = 0; i < (int)m_timages[c].size(); ++i)
//            m_timageall.push_back(m_timages[c][i]);

//        for (int i = 0; i < (int)m_oimages[c].size(); ++i)
//            m_oimagesall.push_back(m_oimages[c][i]);

//    }
//    char buffer[1024];
//    sprintf(buffer, "%sske.dat", m_prefix.c_str());
//    ofstream ofstr;
//    ofstr.open(buffer);
//    ofstr << "SKE" << endl
//          << m_cnum << ' ' << (int)m_timages.size() << endl;

//    for (int c = 0; c < (int)m_timages.size(); ++c) {
//        ofstr << (int)m_timages[c].size() << ' ' << (int)m_oimages[c].size() << endl;

//        for (int i = 0; i < (int)m_timages[c].size(); ++i)
//            ofstr << m_timages[c][i] << ' ';
//        ofstr << endl;
//        for (int i = 0; i < (int)m_oimages[c].size(); ++i)
//            ofstr << m_oimages[c][i] << ' ';
//        ofstr << endl;
//    }
}

void Cbundle::startTimer(void) {
    time(&m_tv);
}

time_t Cbundle::curTimer(void) {
    time(&m_curtime);
    return m_tv - m_curtime;
}
